#include <iostream>
#include "px4_flight_handler.h"

#define PX4_CONTROL_UDP_PORT		14540
#define PX4_CONTROL_UDP_IP			"127.0.0.1"

#define PX4_CONTROL_UART_DEV		"/dev/ttyUSB0"
#define PX4_CONTROL_UART_BAUDRATE	115200

using namespace std;

int main(int argc, char **argv)
{
	cout << "px4 control start..." << endl;

	if(argc < 2)
	{
		cout << "Need set udp or uart!!!" << endl;
		return -1;
	}

	bool ret;

	if(!strcmp(argv[1], "uart"))
	{
		ret = flightHandler.startUpSystemUart(PX4_CONTROL_UART_DEV, PX4_CONTROL_UART_BAUDRATE);
	}
	else if(!strcmp(argv[1], "udp"))
	{
		ret = flightHandler.startUpSystemUdp(PX4_CONTROL_UDP_IP, PX4_CONTROL_UDP_PORT);
	}
	else
	{
		cout << "please set udp or uart!!!!" << endl;
		return -1;
	}

	if(!ret)
	{
		cout << " startUp system error!!!" << endl;
		return -1;
	}

	cout << "connect px4 success." << endl;

	while(1)
	{
		sleepMs(1000);

		//write the cpu temperature to log
		flightHandler.recodeCpuTemperature();

		//fflush log file
		flightHandler.fflushLogFile();

	}
	return true;
}
